#include "bsp.h"
#include "mrfi.h"
#include "nwk_types.h"
#include "nwk_api.h"
#include "bsp_leds.h"
#include "bsp_buttons.h"
#include "vlo_rand.h"

/*------------------------------------------------------------------------------
 * Defines
 *----------------------------------------------------------------------------*/
/* How many times to try a TX and miss an acknowledge before doing a scan */
#define MISSES_IN_A_ROW  2

#define LANE1 BIT0
#define LANE2 BIT1
#define LANE3 BIT2

/*------------------------------------------------------------------------------
 * Prototypes
 *----------------------------------------------------------------------------*/

void createRandomAddress(void);
static void linkTo(void);
__interrupt void Timer_A(void);
__interrupt void Port_2(void);

/*------------------------------------------------------------------------------
 * Globals
 ------------------------------------------------------------------------------*/
static linkID_t sLinkID1 = 0;

/* Initialize radio address location */
char * Flash_Addr = (char *) 0x10F0;

/* Work loop semaphores */

static volatile uint8_t lane = 0; //
/*------------------------------------------------------------------------------
 * Main
 *----------------------------------------------------------------------------*/
void main(void) {
	addr_t lAddr;
	/* Initialize P2 port */
	P2DIR &= ~0x07; // P2.0-P2.2 as input
	P2IE |= 0x07; // P2.0-p2.2 interrupt enabled
	P2IES &= ~0x07; // P2.0-p2.2 lo/Hi edge
	P2IFG &= ~0x07; // P2.0-P2.2 IFG cleared

	//__bis_SR_register(LPM4_bits + GIE); // Enter LPM4 w/interrupt

	/* Initialize board-specific hardware */
	BSP_Init();

	/* Check flash for previously stored address */
	if (Flash_Addr[0] == 0xFF && Flash_Addr[1] == 0xFF && Flash_Addr[2] == 0xFF
			&& Flash_Addr[3] == 0xFF) {
		createRandomAddress(); // Create and store a new random address
	}

	/* Read out address from flash */
	lAddr.addr[0] = Flash_Addr[0];
	lAddr.addr[1] = Flash_Addr[1];
	lAddr.addr[2] = Flash_Addr[2];
	lAddr.addr[3] = Flash_Addr[3];

	/* Tell network stack the device address */
	SMPL_Ioctl(IOCTL_OBJ_ADDR, IOCTL_ACT_SET, &lAddr);

	/* Initialize TimerA and oscillator */
	BCSCTL3 |= LFXT1S_2;                      // LFXT1 = VLO
	TACCTL0 = CCIE;                           // TACCR0 interrupt enabled
	TACCR0 = 12000;                          // ~ 0.1 sec (1000) - 12000== 1 sec
	TACTL = TASSEL_1 + MC_1;                  // ACLK, upmode

	/* Keep trying to join (a side effect of successful initialization) until
	 * successful. Toggle LEDS to indicate that joining has not occurred.
	 */
	while (SMPL_SUCCESS != SMPL_Init(0)) {
		BSP_TOGGLE_LED1()
		;
		BSP_TOGGLE_LED2()
		;
		/* Go to sleep (LPM3 with interrupts enabled)
		 * Timer A0 interrupt will wake CPU up every second to retry initializing
		 */
		__bis_SR_register(LPM3_bits+GIE);
		// LPM3 with interrupts enabled
	}

	/* LEDs on solid to indicate successful join. */
	BSP_TURN_ON_LED1();
	BSP_TURN_ON_LED2();

	/* Unconditional link to AP which is listening due to successful join. */
	linkTo();
	while (1);

}
static void linkTo() {
	uint8_t msg[3];

	/* Keep trying to link... */
	while (SMPL_SUCCESS != SMPL_Link(&sLinkID1)) {
		BSP_TOGGLE_LED1()
		;
		BSP_TOGGLE_LED2()
		;
		/* Go to sleep (LPM3 with interrupts enabled)
		 * Timer A0 interrupt will wake CPU up every second to retry linking
		 */
		__bis_SR_register(LPM3_bits+GIE);
	}

	/* Turn off LEDs. */BSP_TURN_OFF_LED1();
	BSP_TURN_OFF_LED2();

	/* Put the radio to sleep */
	SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SLEEP, 0);

	while (1) {
		/* Go to sleep, waiting for interrupt every second to acquire data */
		__bis_SR_register(LPM3_bits);

		msg[0] = lane;
		msg[1] = 0;
		msg[2] = 0;

		/* Get radio ready...awakens in idle state */
		SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_AWAKE, 0);

		/* No AP acknowledgement, just send a single message to the AP */
		SMPL_SendOpt(sLinkID1, msg, sizeof(msg), SMPL_TXOPTION_NONE );

		/* Put radio back to sleep */
		SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SLEEP, 0);

		msg[0] = 0;


	}
}

void createRandomAddress() {
	unsigned int rand, rand2;
	do {
		rand = TI_getRandomIntegerFromVLO(); // first byte can not be 0x00 of 0xFF
	} while ((rand & 0xFF00) == 0xFF00 || (rand & 0xFF00) == 0x0000);
	rand2 = TI_getRandomIntegerFromVLO();

	BCSCTL1 = CALBC1_1MHZ;                    // Set DCO to 1MHz
	DCOCTL = CALDCO_1MHZ;
	FCTL2 = FWKEY + FSSEL0 + FN1;           // MCLK/3 for Flash Timing Generator
	FCTL3 = FWKEY + LOCKA;                    // Clear LOCK & LOCKA bits
	FCTL1 = FWKEY + WRT;                      // Set WRT bit for write operation

	Flash_Addr[0] = (rand >> 8) & 0xFF;
	Flash_Addr[1] = rand & 0xFF;
	Flash_Addr[2] = (rand2 >> 8) & 0xFF;
	Flash_Addr[3] = rand2 & 0xFF;

	FCTL1 = FWKEY;                            // Clear WRT bit
	FCTL3 = FWKEY + LOCKA + LOCK;             // Set LOCK & LOCKA bit
}

/*------------------------------------------------------------------------------
 * Port 2 interrupt service routine
 *-------------------------------------------------------------------------------*/
#pragma vector=PORT2_VECTOR
__interrupt void Port_2(void) {
	BSP_TOGGLE_LED1();
	if (P2IFG & LANE1) {
		// Fare qualcosa
		lane = 1;
		P2IFG &= ~LANE1; // clear interrupt
	}
	if (P2IFG & LANE2) {
		lane = 2;
		P2IFG &= ~LANE2; // clear interrupt
	}
	if (P2IFG & LANE3) {
		lane = 3;
		P2IFG &= ~LANE3; // clear interrupt
	}

	MRFI_GpioIsr();
	__bic_SR_register_on_exit(LPM3_bits);
}

/*------------------------------------------------------------------------------
 * Timer A0 interrupt service routine
 *----------------------------------------------------------------------------*/
#pragma vector=TIMERA0_VECTOR
__interrupt void Timer_A(void) {
	lane=0;
	__bic_SR_register_on_exit(LPM3_bits);

}
